
#include "Matrix4x4.h"
#include "Vector.h"
#include <math.h>
#include "../Helix.h"
#include "../HObject.h"
#include "../Log.h"

namespace helix
{

	Matrix4x4::Matrix4x4()
	{
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				if(i==j)
					mat[i][j]=1.0;
				else
					mat[i][j]=0.0;
			}
		}
	}

	Matrix4x4::Matrix4x4(double mat[4][4])
	{
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				this->mat[i][j]=mat[i][j];
			}
		}
	}

	Matrix4x4::Matrix4x4(float mat[4][4])    //create a matrix from a 2d array of float values
	{
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				this->mat[i][j]=(double)mat[i][j];
			}
		}
	}


	Matrix4x4::Matrix4x4(double *mat)		//same as above, but from a pointer to the array
	{
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				this->mat[i][j]=mat[i*4+j];
			}
		}
	}

	Matrix4x4::Matrix4x4(double transx,double transy,double transz)	//create a translation matrix
	{
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<3;j++)
			{
				if(i==j)
					this->mat[i][j]=1.0;
				else
					this->mat[i][j]=0.0;
			}
		}
		this->mat[0][3]=transx;
		this->mat[1][3]=transy;
		this->mat[2][3]=transz;
		this->mat[3][3]=1.0;
	}

	Matrix4x4::Matrix4x4(double degrees,int axis)	//create a rotation matrix
	{
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				if(i==j)
					mat[i][j]=1.0;
				else
					mat[i][j]=0.0;
			}
		}
		double rads=PI_OVER_180*degrees;
		double x1=cos(rads);
		double x2=sin(rads);
		switch(axis)
		{
			case H_X_AXIS: mat[1][1]=x1;
						mat[1][2]=-x2;
						mat[2][1]=x2;
						mat[2][2]=x1;
						break;
			case H_Y_AXIS: mat[0][0]=x1;
						mat[0][2]=x2;
						mat[2][0]=-x2;
						mat[2][2]=x1;
						break;
			case H_Z_AXIS: mat[0][0]=x1;
						mat[0][1]=-x2;
						mat[1][0]=x2;
						mat[1][1]=x1;
						break;
		}
	}

	Matrix4x4::Matrix4x4(double scalex,double scaley,double scalez,double scalefactor) //create a scale matrix
	{
		scalex*=scalefactor;
		scaley*=scalefactor;
		scalez*=scalefactor;
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				mat[i][j]=0.0;
			}
		}

		mat[0][0]=scalex;
		mat[1][1]=scaley;
		mat[2][2]=scalez;
		mat[3][3]=1.0;
	}

	Matrix4x4::~Matrix4x4()
	{
	}

	Matrix4x4 Matrix4x4::operator + (Matrix4x4 &M)	//overload the + operator to add matrices
	{
		Matrix4x4 NewMat=Matrix4x4();
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				NewMat.mat[i][j]=mat[i][j]+M.mat[i][j];
			}
		}
		return NewMat;
	}

	Matrix4x4 Matrix4x4::operator += (Matrix4x4 &M) //same
	{
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				mat[i][j]+=M.mat[i][j];
			}
		}
		return *this;
	}


	Matrix4x4 Matrix4x4::operator * (double &scalar) //multiply matrix by a scalar
	{
		Matrix4x4 rmat=Matrix4x4();
		for(int i=0;i<3;i++)
		{
			for(int j=0;j<3;j++)
			{
				rmat.mat[i][j]*=scalar;
			}
		}
		return rmat;
	}

	Matrix4x4 Matrix4x4::operator / (double &scalar)	//divide matrix by a scalar
	{
		Matrix4x4 rmat=Matrix4x4();
		for(int i=0;i<3;i++)
		{
			for(int j=0;j<3;j++)
			{
				rmat.mat[i][j]/=scalar;
			}
		}
		return rmat;
	}


	Vector Matrix4x4::operator * (Vector &vector) //multiply matrix by a vector
	{
		double origvec[4]={vector.x(),vector.y(),vector.z(),1.0};
		double vec[4]={0,0,0,0};
	    
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				vec[i]+=origvec[j]*mat[i][j];   //dot product each row of the matrix with the vector
			}
		}
	//	if(vec[3]!=1.0)
	//	{
	//		for(int i=0;i<4;i++)
	//			vec[i]/=vec[3];
	//	}
	    
		return Vector((float)vec[0],(float)vec[1],(float)vec[2]);
	}

	Matrix4x4 Matrix4x4::operator * (Matrix4x4 &M)   //multiply matrix by another matrix
	{
		Matrix4x4 matrix=Matrix4x4();
		for(int i=0;i<4;i++)
			for(int j=0;j<4;j++)
				matrix.mat[i][j]=0.0;

		for(int i = 0;i < 4;i++)
		{
			for(int j = 0;j < 4;j++)
			{
				for(int k = 0;k < 4;k++)
				{
					matrix.mat[i][j] += mat[i][k]*M.mat[k][j];
				}
			}
		}

		return matrix;
	}


	Matrix4x4 Matrix4x4::Inverse()    //returns the inverse matrix of this matrix
	{
		Matrix4x4 rmat=Matrix4x4();	//create a new identity matrix
		Matrix4x4 tmat=Matrix4x4(this->getMatrix());

		double r[4];
		for(int k=0;k<4;k++)
		{
			r[k]=tmat.mat[k][k];				//get the first value of the first row
			for(int i=0;i<4;i++)
			{
				tmat.mat[k][i]/=r[k];
				rmat.mat[k][i]/=r[k];
			}
			r[1]=-tmat.mat[1][k];
			r[2]=-tmat.mat[2][k];
			r[3]=-tmat.mat[3][k];
			for(int i=0;i<4;i++)
			{
				if(i!=k)
				{
					for(int j=0;j<4;j++)
					{
						tmat.mat[i][j]+=r[i]*tmat.mat[i][j];
						rmat.mat[i][j]+=r[i]*rmat.mat[i][j];
					}
				}
			}
		}	

		return rmat;
	}

	Matrix4x4 Matrix4x4::Transpose()		//returns the transpose matrix of this matrix
	{
		Matrix4x4 rmat=Matrix4x4();
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				rmat.mat[i][j]=mat[j][i];
			}
		}
		return rmat;
	}

	Matrix4x4 Matrix4x4::InverseTranspose()	//returns the inverse transpose of this matrix
	{
		Matrix4x4 rmat=this->Inverse();
		return rmat.Transpose();
	}

	float Matrix4x4::Determinant()			//returns the matrix's determinant
	{
		return 1.0f;		//does nothing for now
	}

	float Matrix4x4::Determinant3x3()		//returns the determinant of the upper left 3x3 matrix's determinant
	{
		float rValue=0.0f;

		rValue=(float)(mat[0][0]*(mat[1][1]*mat[2][2] - mat[2][1]*mat[1][2]) -
			mat[0][1]*(mat[1][0]*mat[2][2] - mat[2][0]*mat[1][2]) +
			mat[0][2]*(mat[1][0]*mat[2][1] - mat[1][1]*mat[2][0]));

		return rValue;
	}



	void Matrix4x4::MakeIdentity()   //makes this matrix an identity matrix
	{
		for(int i=0;i<4;i++)
		{
			for(int j=0;j<4;j++)
			{
				if(i==j)
					mat[i][j]=1.0;
				else
					mat[i][j]=0.0;
			}
		}
	}

	void Matrix4x4::setRow(int row,Vector vect)	//set a certain row with a vector (leaves mat[row][3] alone)
	{
		if(row<0 || row>3)
			return;

		mat[row][0]=vect.x();
		mat[row][1]=vect.y();
		mat[row][2]=vect.z();
	}

	void Matrix4x4::setRow(int row,Vector vect,double last)	//same as above, but lets you set  mat[row][3]
	{
		if(row<0 || row>3)
			return;

		mat[row][0]=vect.x();
		mat[row][1]=vect.y();
		mat[row][2]=vect.z();
		mat[row][3]=last;
	}

	void Matrix4x4::setCol(int col,Vector vect)	//set a column with a vector
	{
		if(col<0 || col>3)
			return;

		mat[0][col]=vect.x();
		mat[1][col]=vect.y();
		mat[2][col]=vect.z();
	}

	void Matrix4x4::setCol(int col,Vector vect,double last)	//same as above, but lets you set mat[3][col]
	{
		if(col<0 || col>3)
			return;

		mat[0][col]=vect.x();
		mat[1][col]=vect.y();
		mat[2][col]=vect.z();
		mat[3][col]=last;
	}

	double* Matrix4x4::getMatrix()
	{
		return &mat[0][0];
	}

	void Matrix4x4::Print()
	{
		Log().Println("   Matrix: |%.3f %.3f %.3f %.3f|",mat[0][0],mat[0][1],mat[0][2],mat[0][3]);
		Log().Println("           |%.3f %.3f %.3f %.3f|",mat[1][0],mat[1][1],mat[1][2],mat[1][3]);
		Log().Println("           |%.3f %.3f %.3f %.3f|",mat[2][0],mat[2][1],mat[2][2],mat[2][3]);
		Log().Println("           |%.3f %.3f %.3f %.3f|",mat[3][0],mat[3][1],mat[3][2],mat[3][3]);
	}

}